module imhoff
implicit none
contains
subroutine imhofprob(pr,ifault,mu0,A,P,c)
!Probability that x'Px < c where x is N(mu,A)
use linear_operators
use matrixfuns
use lin_eig_self_int 
use rand_gen_int 


double precision, intent(in):: mu0(:,:),A(:,:),P(:,:),c
double precision, intent(out)::pr
integer, intent(out)::ifault
double precision:: RA(1:size(A,1),1:size(A,2)),mu(1:size(mu0,1),1:1)&
&,Q(1:size(A,1),1:size(A,2)),H(1:size(A,1),1:size(A,2)),Qprev(1:size(A,1),1:size(A,2))&
&,check, D(1:size(A,1)),check1,check2,v(1:size(A,1),1:1)&
&,lambda(1:size(A,1))
double precision::mult(1:size(A,1)),delta(1:size(A,1)),arg,bound,eps1,eps2,eps3&
&,dmach,tol
integer::noterms,fil,col,irank




tol = 100.0*dmach(4)
call dchfac(size(A,1),A,size(A,1),tol,irank,RA,size(RA,1))
RA=.t.RA

!RA=.t.chol(A)



mu=(.i.RA).x.mu0
Q=(RA.tx.P).x.RA
fil=size(Q,1)
col=size(Q,2)

call lin_eig_self(Q,D,v=H)


check=maxmax((H.xt.H)-eye(fil))
if (check.gt.1.0d-4) then
    Qprev=Q
    Q=floor(1.0d6*Q)/1.0d6 
	call lin_eig_self(Q,D,v=H)
    check1=maxmax(dabs(Q-Qprev))
    check2=maxmax(dabs((H.xt.H)-eye(fil)))
    check=max(check1,check2)
    if (check.gt.1.0d-4) then
		print*, "Warning:H may not be orthogonal"
	    print*, "H"
	    print*, H
    end if
end if
v=H.tx.mu
lambda=D
mult=vassign(fil,1.0d0)
v=mhadamard(v,v)
delta=v(:,1)
noterms=fil
arg=c
!eps1=1.0d-8
!eps2=1.0d-7
!eps3=1.0d-8

eps1=1.0d-5
eps2=1.0d-4
eps3=1.0d-5

bound=-10.0d0
call imhof(pr,ifault,lambda,mult,delta,noterms,arg,bound,eps1,eps2,eps3);
end subroutine imhofprob
!!
subroutine imhof(x,ifault,lambda,mult,delta,noterms,arg,bound,eps1,eps2,eps3)
!
! {Algorithm AS 256.3  Appl. Statist. (1990) Vol.39, No.2}
!
! {Pascal translation of Koerts and Abrahamse's implementation of
!  Imhof's procedure for evaluating the probability that a diagonal
!  quadratic form in normal variables is less than a given value, arg}

! lambda contains diagonal elements of diagonal matrix in quadratic form
! mult contains multiplicities of diagonal elements
! delta contains means of standard normals (for non-central cases)
! noterms contains dimension of lambda, delta
double precision::lambda(:),mult(:),delta(:),arg,bound,eps1,eps2,eps3
integer::noterms
integer, intent(out)::ifault
double precision, intent(out):: x
double precision, parameter:: pi=3.14159265358979
integer i


!{check parameters}
if ((noterms .lt. 1) .or. (eps1 .le. 0.0) .or. (eps2 .le. 0.0) .or. (eps3 .le. 0.0)) then
   ifault = 2
   x = -2.0d0
else
   ifault = 1
   i = 0  !% correction?
   do while (ifault==1)
      i = i+1
      if (i==noterms) then
         ifault = 0
      end if
      if ((mult(i) .lt. 1) .or. (delta(i) .lt. 0.0)) then
         ifault = 3
         x = -i
      end if
   end do

!  {main body}
   if (ifault==0) then
      if (bound .le. 0.0) then  ! one may choose the bound rather than use:
		 call imhofbd(bound,ifault,lambda,mult,delta,noterms,eps1,eps2)
      end if
      if (ifault==0) then
		 call imhofint(x,ifault,lambda,mult,delta,noterms,arg,bound,eps3)
      else
         x = -ifault
      end if
   end if
end if

end subroutine imhof
!!
subroutine imhofbd(bd,ifault,lambda,mult,delta,noterms,eps1,eps2)
! {Algorithm AS 256.1  Appl. Statist. (1990) Vol.39, No.2}
!
! {Pascal translation of Koerts and Abrahamse's procedure
!   for evaluating Imhof's upper bound}
!
! matlab translation:  par 5/20/98
!
! lambda contains diagonal elements of diagonal matrix in quadratic form
! mult contains multiplicities of diagonal elements
! delta contains means of standard normals (for non-central cases)
! noterms contains dimension of lambda, delta
double precision, intent(in)::lambda(:),mult(:),delta(:),eps1,eps2
integer, intent(in)::noterms
integer, intent(out)::ifault
double precision, intent(out):: bd
double precision sum1,sum2,hold,count,range
double precision, parameter:: pi=3.14159265358979
integer i

ifault=0
if ((noterms.lt.1) .or. (eps1 .le.0.0) .or. (eps2.le.0.0)) then
   ifault  =  2
   bd = -2.0d0
else
   count = 0
   sum1  = 0.0d0
   sum2  = 0.0d0
   do i = 1,noterms,1
      hold = dabs(lambda(i))
      if (hold > eps2) then
         count = count+mult(i)
         sum1 = sum1+mult(i)*dlog(hold)
      end if
      sum2 = sum2+delta(i)
   end do
   if (count < 0.9) then
      ifault = 4
      bd = -4.0d0
   else
      count = 0.5*count
      sum1 = 0.5*sum1+dlog(pi*count)
      range = dexp(-(sum1+0.5*sum2+dlog(eps1))/count)
      if (sum2==0.0) then
         range = range+5.0/count
      else
         do while (count.ne.0.0)
            sum2 = 0.0
            do i = 1,noterms,1
               hold = (range*lambda(i))*(range*lambda(i))
               sum2 = sum2+delta(i)*hold/(1.0+hold)
            end do
            hold = dexp(sum1+count*dlog(range)+0.5*sum2)
            if (hold*eps1 <= 1.0) then
               range = range+5.0/count
            else
               count = 0.0
            end if
         end do
      end if
   end if
   bd = range
end if

end subroutine imhofbd
!!
subroutine imhofint(x,ifault,lambda, mult, delta, noterms, arg, bound, eps3)
!
! {Algorithm AS 256.2  Appl. Statist. (1990) Vol.39, No.2}
!
! {Pascal translation of Koerts and Abrahamse's procedure
!  for evaluating Imhof's integral by Simpson's Rule}

double precision, intent(in)::lambda(:),mult(:),delta(:),arg,bound,eps3
integer, intent(in)::noterms
integer, intent(out)::ifault
double precision, intent(out):: x
double precision step,eps4,sum1,sum2,sum3,sum4,int2,int1
double precision, parameter:: pi=3.14159265358979
integer i,j,maxit,n,cgd



maxit=30

if ((noterms .lt. 1) .or. (bound .le. 0.0) .or. (eps3 .le. 0.0)) then
   ifault = 2
   x = -2.0
else
   ifault = 5
   n = 2
   step = 0.5*bound
   eps4 = 3.0*pi*eps3
   sum1 = -arg
   do i = 1,noterms,1
      sum1 = sum1+lambda(i)*(mult(i)+delta(i))
   end do
   sum1 = 0.5*sum1+imhoffn(bound,lambda,mult,delta,noterms,arg,bound,eps3)
   sum2 = 0.0
   sum4 = imhoffn(step,lambda,mult,delta,noterms,arg,bound,eps3)
   int2 = (sum1+4.0*sum4)*step
   i = 0
	cgd = 0
   do while ((i.ne.maxit) .and. (cgd==0))
      i = i+1
      n = n+n
      step = 0.5*step
      int1 = int2
      sum2 = sum2+sum4
      int2 = sum1+sum2+sum2
      sum4 = 0.0
      j = 1
      do while (j .le. n)
         sum4 = sum4+imhoffn(j*step,lambda,mult,delta,noterms,arg,bound,eps3)
         j = j+2
      end do
      int2 = (int2+4.0*sum4)*step
      if (i > 3) then
         if (dabs(int1-int2) < eps4) then
            if (dabs(int2) > 1.5*pi) then
               ifault = 6
            else
               ifault = 0
            end if
            cgd = 1
         end if
      end if
   end do
   x = 0.5-int2/(3.0*pi)
end if
end subroutine imhofint
!!
function imhoffn(u, lambda, mult, delta, noterms, arg, bound, eps3)
double precision u,lambda(:),mult(:),delta(:),arg,bound,eps3,imhoffn
double precision rho,sum,theta,hold,hold2,hold3
integer noterms,i
!
!   {imhoffn evaluates Imhof's integrand}
!

rho   = 0.0
sum   = 0.0
theta = -u*arg
do i = 1,noterms,1
   hold  = u*lambda(i)
   hold2 = hold*hold
   hold3 = 1.0+hold2
   theta = theta+mult(i)*datan(hold)+delta(i)*hold/hold3
   sum   = sum+delta(i)*hold2/hold3
   rho   = rho+mult(i)*dlog(hold3)
end do
imhoffn = dsin(0.5*theta)/(u*dexp(0.5*sum+0.25*rho))
end function imhoffn
end module imhoff